/*
 * Copyright (c) 2010 Network Security Lab (NSL), Department of Electrical
 *                    Engineering, University of Washington
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Author: He Wu mdzz@uw.edu
 */

/**
 * @file command.c
 */

#include <unistd.h>
#include <pthread.h>
#include <fcntl.h>
#include <string.h>
#include <pthread.h>
#include <stdio.h>
#include "command.h"
#include "command_def.h"
#include "router.h"
#include "localization.h"
#include "socket.h"
#include "sensor.h"

#define BUF_SIZE 4  //!< buffer size for led commands

// debug switch
#define cmd_debug 0

// global for the GUI
__s8 total_cmds = 0;  //!< total number of commands to be sent out by the base node
__s8 sent_cmds = 0;   //!< number of commands sent by the base node

// global
static int list_index = 0;      //!< index to seq_list
static seq_num_record seq_list[MAX_SEQ];  //!< list of previously received packet sequence number
pthread_mutex_t seqListLock = PTHREAD_MUTEX_INITIALIZER;
int restart_flag = 0;           //!< flag for restarting program

/******************************************************************************/

/**
 * @brief Handles command packets.
 * @param in_pkt Pointer to packets received.
 * @return 0 if all is good.
 */
static int cmd_handler(packet *in_pkt);

/******************************************************************************/

void init_seq_list()
{
  int i = 0;
  for (i = 0; i < MAX_SEQ; i++) {
    seq_list[i].addr = 0;
    seq_list[i].seq = 0;
  }
}


void pkt_processor(packet *in_pkt)
{
  int i;

  // check if packet is previously received, skipping broadcast/routing packets
  if ((in_pkt->type != ANCHOR_BROADCAST) && (in_pkt->type != ROUTING_PKT_TYPE)
      && (in_pkt->type!= TO_NODE_ROUTING_PKT_TYPE)) {
    pthread_mutex_lock(&seqListLock);
    for (i = 0; i < MAX_SEQ; i++) {
      if ((seq_list[i].seq == in_pkt->seq) && (seq_list[i].addr == in_pkt->senderAddr)) {
        pthread_mutex_unlock(&seqListLock);
        printf("\nThis packet has been received before!\n\n");
        return;
      }
    }
    seq_list[list_index].seq = in_pkt->seq;
    seq_list[list_index].addr = in_pkt->senderAddr;
    if (++list_index > MAX_SEQ)
      list_index = 0; // reset list index
    pthread_mutex_unlock(&seqListLock);
  }

  // process different packets
  switch (in_pkt->type) {
  case CMD_PKT_TYPE:
    printf("Command packet.\n");
    forward_to_node(in_pkt, 1); // ACK on
    break;
  case ANCHOR_BROADCAST:
    printf("Anchor broadcast.\n");
    cmd_handler(in_pkt);
    break;
  case ROUTING_PKT_TYPE:
    printf("Node-to-base routing packet.\n");
    recv_rtPkt(in_pkt);
    break;
  case TO_NODE_ROUTING_PKT_TYPE:
    printf("Base-to-node routing packet.\n");
    forward_rt_toBase(in_pkt);
    break;
  case TO_BASE_DATA_PKT_TYPE:
    printf("Location data packet.\n");
    forward_to_base(in_pkt, 1); // ACK on
    break;
  case SENSOR_PKT_TYPE:
    printf("Sensor packet.\n");
    sensor_pkt_handler(in_pkt);
    break;
  case HEALTH_DATA_TYPE:
    printf("Health data packet.");
    forward_to_base(in_pkt, 1); // ACK on
    break;
  default:
    printf("Undefined packet type.\n");
    break;
  }
}


int forward_to_base(packet *out_pkt, int ACK_en)
{
  __u16 localAddr; // local address

  // read local address form config file
  localAddr = get_addr();

  if (check_base()) {
    printf("\nPacket has reached base!\n\n");
    // call cmd handler
    cmd_handler(out_pkt);
    return 0;
  }
  else {
    out_pkt->senderAddr = localAddr;
    if (sendtoBase(out_pkt, ACK_en) < 0)
      return -1;
  }
  return 0;
}


int forward_to_node(packet *out_pkt, int ACK_en)
{
  __u16 localAddr; // local address
  __u16 destAddr;
  __u16 nextHop;

  // read local address form config file
  localAddr = get_addr();

  // get dest address
  destAddr = out_pkt->data[1];
  destAddr = destAddr << 8;
  destAddr += out_pkt->data[2];

  if (localAddr == destAddr) { // packet arrived at destination
    printf("\nPacket has reached destination node!\n\n");
    // re-construct cmd content
    memcpy(out_pkt->data, out_pkt->data + 14, 12);
    // call cmd handler
    cmd_handler(out_pkt);
    return 0;
  }
  else {
    out_pkt->data[0]--; // update number of hops left
    out_pkt->senderAddr = localAddr;
    // get next hop from the end of the list
    nextHop = out_pkt->data[1 + 2 * (out_pkt->data[0] - 1)];
    nextHop = nextHop << 8;
    nextHop += out_pkt->data[2 + 2 * (out_pkt->data[0] - 1)];
    // send pkt
    printf("Sending packet to %x through: %x\n", destAddr, nextHop);
    return h_send(nextHop, out_pkt, ACK_en);
  }
  return 0;
}


void toggle_green()
{
  int gpioControl = open(GPIO_CONTROL, O_WRONLY | O_NOCTTY | O_NDELAY);
  write(gpioControl, "r104", 3);
  close(gpioControl);
  int green = open(GPIOGREEN, O_RDONLY | O_NOCTTY | O_NDELAY);
  char buffer[BUF_SIZE];
  read(green, buffer, BUF_SIZE - 1);
  close(green);
  int number = atoi(buffer);
  if (number == 1)
    system("echo c104 > /proc/gpio/GPCTL");
  else
    system("echo s104 > /proc/gpio/GPCTL");
}


void on_green()
{
  system("echo c104 > /proc/gpio/GPCTL");
}


void off_green()
{
  system("echo s104 > /proc/gpio/GPCTL");
}


void toggle_red()
{
  int gpioControl;
  gpioControl = open(GPIO_CONTROL, O_WRONLY | O_NOCTTY | O_NDELAY);
  write(gpioControl, "r103", 3);
  close(gpioControl);
  int red;
  char buffer[BUF_SIZE];
  red = open(GPIORED, O_RDONLY | O_NOCTTY | O_NDELAY);
  read(red, buffer, BUF_SIZE - 1);
  close(red);
  int number = atoi(buffer);
  if (number == 1)
    system("echo c103 > /proc/gpio/GPCTL");
  else
    system("echo s103 > /proc/gpio/GPCTL");
}


void on_red()
{
  system("echo c103 > /proc/gpio/GPCTL");
}


void off_red()
{
  system("echo s103 > /proc/gpio/GPCTL");
}


void toggle_blue()
{
  int gpioControl;
  gpioControl = open(GPIO_CONTROL, O_WRONLY | O_NOCTTY | O_NDELAY);
  write(gpioControl, "r105", 3);
  close(gpioControl);
  int blue = open(GPIOBLUE, O_RDONLY | O_NOCTTY | O_NDELAY);
  char buffer[BUF_SIZE];
  read(blue, buffer, BUF_SIZE - 1);
  close(blue);
  int number = atoi(buffer);
  if (number == 1)
    system("echo c105 > /proc/gpio/GPCTL");
  else
    system("echo s105 > /proc/gpio/GPCTL");
}


void on_blue()
{
  system("echo c105 > /proc/gpio/GPCTL");
}


void off_blue()
{
  system("echo s105 > /proc/gpio/GPCTL");
}


int send_cmd(__u8 *cmd_list)
{
  packet out_pkt;
  __u8 *tmp = cmd_list + 1; // ignore the first byte of the command list
  __u16 destAddr;
  int i;

  // initialize packet
  initPkt(&out_pkt, CMD_PKT_TYPE);

  if (cmd_debug)
    printf("Number of commands to be sent is: %d\n", total_cmds);

  // send all command packets
  for (i = 0; i < total_cmds; i++) {
    sent_cmds = i;  // update number of commands left to send
    memcpy(&destAddr, tmp, 2);
    memcpy(out_pkt.data, tmp + 2, 12);  // get command content
    tmp += 14;                          // move to next command
    if (cmd_debug) {
      printf("Command should be sent to %x\n", destAddr);
      printf("Command is %d\n", out_pkt.data[1]);
    }
    printf("\nSending packet #%d, %d left to send.\n\n", i, total_cmds - i);

    // send packet via routing
    if (sendtoNode(destAddr, &out_pkt, 1) < 0) {
      printf("Error sending command to Node: %x\n", destAddr);
      errorSocket(out_pkt); // report error packet back to GUI
    }
  }

  // reset counter
  sent_cmds = 0;
  total_cmds = 0;

  printf("All commands sent!!!\n\n");

  return 0;
}

/******************************************************************************/

static int cmd_handler(packet *in_pkt)
{
  printf("Received command # is: %d\n", in_pkt->data[1]);
  switch (in_pkt->data[1]) {
  case BLUE_TOGGLE:
    printf("Toggle blue LED.\n");
    toggle_blue();
    break;
  case BLUE_ON:
    printf("Turn on blue LED.\n");
    on_blue();
    break;
  case BLUE_OFF:
    printf("Turn off blue LED.\n");
    off_blue();
    break;
  case RED_TOGGLE:
    printf("Toggle red LED.\n");
    toggle_red();
    break;
  case RED_ON:
    printf("Turn on red LED.\n");
    on_red();
    break;
  case RED_OFF:
    printf("Turn off red LED.\n");
    off_red();
    break;
  case GREEN_TOGGLE:
    printf("Toggle green LED.\n");
    toggle_green();
    break;
  case GREEN_ON:
    printf("Turn on green LED.\n");
    on_green();
    break;
  case GREEN_OFF:
    printf("Turn off green LED.\n");
    off_green();
    break;
  case ALL_TOGGLE:
    printf("Toggle all LEDs.\n");
    toggle_red();
    toggle_green();
    toggle_blue();
    break;
  case ALL_ON:
    printf("Turn on all LEDs.\n");
    on_red();
    on_green();
    on_blue();
    break;
  case ALL_OFF:
    printf("Turn off all LEDs.\n");
    off_red();
    off_green();
    off_blue();
    break;
  case LOC_DATA:
  case EST_LOC_REPORT:
  case ACTUAL_LOC_REPORT:
    if (check_base()) {
      printf("Reporting location to GUI!\n\n");
      dataSocket(*in_pkt);
      break;  // base node will ignore all other localization related commands
    }
  case ASSIGN_XY:
  case LOC_OFF:
  case PROPOGATE_ON:
  case PROPOGATE_OFF:
  case LOC_ON:
  case CENTROID:
  case WEIGHTEDCENTROID:
  case GRIDSCORE:
  case WEIGHTED_SCORE:
  case GRADIENT:
  case WEIGHTED_GRADIENT:
  case CHANGE_MAX_RSSI:
  case CHANGE_LOCAL_TIMER_INTERVAL:
    if (!check_base()) {
      printf("Handling localization packets.\n");
      localization_msg(in_pkt);
    }
    break;
  case RESTART:
    if (!check_base()) {
      printf("Restarting!\n");
      restart_flag = 1;
      break;
    }
    printf("Base node, ignoring restart command!\n");
    break;
  case HEALTH_DATA:
    printf("Handling health data packets.\n");
    health_pkt_handler(in_pkt);
    dataSocket(*in_pkt);
    break;
  default:
    printf("Undefined command # %d.\n", in_pkt->data[1]);
    return -1;
  }
  return 0;
}
